#include "sensor.h"
#include "math.h"
float offset = 0.0;
	uint8_t sensor_arry[9]={0,0,0,0,0,0,0,0,0};//传感器数组
	uint8_t tracking_move = 2; //1: A -> B  2: A -> B -> C -> D  3: 8字环绕
	uint8_t stop_flag = 1; // 停止标志位 1-停止 0-运行
	uint8_t path_state = 1; // 路径状态

	void sensor_read(uint8_t *sensor_arry)
	{
	    if(sensor_arry == NULL) 
			return; // 检查指针是否为NULL
	    
		sensor_arry[1] = (HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_14) == GPIO_PIN_SET) ? 1 : 0; // 左3
		sensor_arry[2] = (HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_15) == GPIO_PIN_SET) ? 1 : 0; // 左2
		sensor_arry[3] = (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2) == GPIO_PIN_SET) ? 1 : 0; // 左1
		sensor_arry[4] = (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_3) == GPIO_PIN_SET) ? 1 : 0; // 中间
		sensor_arry[5] = (HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_1) == GPIO_PIN_SET) ? 1 : 0; // 右1
		sensor_arry[6] = (HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_0) == GPIO_PIN_SET) ? 1 : 0; // 右2
		sensor_arry[7] = (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_8) == GPIO_PIN_SET) ? 1 : 0; // 右3
	}

	void Sensor_TaskWrapper(void)
	{
	    sensor_read(sensor_arry); // 读取传感器状态
	}
    
void motor_speed_set(int16_t left_speed, int16_t right_speed)
{	    	
	if(left_speed >=0)
	{
	// 电机方向的设置
		MOTOR_A_FORWARD();  // 左电机
//	
	}
	else if(left_speed <0)
	{
		left_speed=-left_speed;
		MOTOR_A_BACKWARD();
	
	}
	if(right_speed >=0)
	{
		MOTOR_B_FORWARD();  // 右电机
	}
	else if(right_speed <0)
	{
		right_speed=-right_speed;
		MOTOR_B_BACKWARD();  // 右电机

	}
	// 设置电机速度
	Motor_SetSpeed(1, left_speed);   // 1是左电机
	Motor_SetSpeed(2, right_speed);  // 2是右电机

	    
}  
// 巡线PID参数
void sensor_track_PID_Init(void)
{
	sensor_track_PID.Kp = 0.00f;
	sensor_track_PID.Ki = 0.00f;
	sensor_track_PID.Kd = 0.00f;
	sensor_track_PID.integral_max = 50.0f;
}

//计算七路传感器偏移量。 其中1代表黑线，0代表白色地图背景，数组下标从0开始
float cal_sensor_offset(void)
{
//	float offset = 100.0f;
	uint8_t sum = 0;
	for (int i = 1; i <= 7; i++)
	{
		sum += sensor_arry[i];
	}
	uint8_t black = sum;
	//printf("%d\n",black);
	if(black == 0)
		offset = 100.0f;
	else if (black == 2) //检测到相邻2个传感器在黑线上
	{
		for (int i = 1; i <= 6; i++)
		{
			if (sensor_arry[i] == 1 && sensor_arry[i + 1] == 1)
			{
				offset = (i + 0.5f) - 3.0f; // 计算偏移量
			}
		}
	}
	else if (black >= 3) // 检测到3个及以上传感器在黑线上
	{
       // 使用加权平均计算偏移量
        float total = 0.0f;
        float weight = 0.0f;
		const float weights[8] = {0, 1.0, 1.2, 1.5, 1.8, 1.5, 1.2, 1.0};
        for (int i = 1; i <= 7; i++)
        {
            if (sensor_arry[i] == 1) // 黑线
            {
                total += (i - 4) * weights[i]; // 以传感器4为原点
                weight += weights[i];
            }
		}
		offset = total / weight;
	}
	else if (black == 1) // 只检测到1个传感器在黑线上
	{
		for (int i = 1; i <= 7; i++)
		{
			if (sensor_arry[i] == 1)
			{
				offset = (float)(i - 4) * 1.5f; // 计算偏移量
				
				
			}
		}
	}
//	printf("off %.2f\n",offset);
	return offset; // 返回偏移量

}

/*-----------------------------------------循迹任务-------------------------------------------------*/

void tarcking_task(void)
{
//	printf("%.2f,%.2f,%.2f\n",sensor_track_PID.Kp,sensor_track_PID.Ki,sensor_track_PID.Kd);
	float offset = cal_sensor_offset(); // 计算传感器偏移量
	float pid_output =  0;
	static uint8_t last_on_line = 0;
	uint8_t on_line  = (sensor_arry[1] == 1 || sensor_arry[2] == 1 || sensor_arry[3] == 1 || sensor_arry[4] == 1 || sensor_arry[5] == 1 || sensor_arry[6] == 1 || sensor_arry[7] == 1);

//	if(tracking_move == 1) // A -> B
//	{			
//		// 扫到黑线就停车
//		if (on_line)
//		{
//			motor_speed_set(0, 0); // 停车
//			stop_flag = 1;
//			//白进黑
//			if(last_on_line == 0)
//			{
//				HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,GPIO_PIN_RESET);
//				 buzzer_beep(1,300,0);  // 300ms鸣响
//			}
//		}
//		else
//		{
//			// 没有黑线时直走
//			jy62_proc();
//			stop_flag = 0;
//		}
//		last_on_line = on_line;
//	}
//	if(tracking_move == 2)//A -> B -> C -> D
//	{
//		int32_t base_speed =0; // 动态调整基础速度
//		int32_t L_P = 0;
//		int32_t R_P = 0;
//		switch(path_state)
//		{
//			case 1: //直走状态1
//				if(offset != 100.0f)
//				{
//					path_state	= 2;
//					JY62_PID.error_last = 0;
//					JY62_PID.integral = 0;
//					buzzer_beep(1,300,100);
//					HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,GPIO_PIN_RESET);

//				}
//				else
///					jy62_proc();
//						
//				break;
//			case 2: //循迹状态1
//				if(offset == 100.0f)
//				{
//					path_state = 3;
//					Yaw_target = 180.0f; // 设置出口角度
//					JY62_PID.error_last = 0;
//					JY62_PID.integral = 0;
//					buzzer_beep(1,300,100);
//					HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_RESET);
//					HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,GPIO_PIN_SET);

//				}
//				else
//				{	
//					base_speed= 350 - fabs(offset) * 0.5;
//					if (base_speed < 200) base_speed = 200;
//					pid_output = PID_Calc(&sensor_track_PID, 0 , offset,0);
//					L_P = base_speed - pid_output;
//					R_P = base_speed + pid_output;
//					motor_speed_set(L_P, R_P);
////					printf("%d,%d\n",L_P,R_P);
//					return;
//				}
//			break;
//			case 3://直走状态2
//			
//					if(offset != 100.0f) // 检测到黑线
//					{
//						path_state = 4;
//						buzzer_beep(1,300,100);
//						HAL_GPIO_WritePin(GPIOB,GPIO_PIN_14,GPIO_PIN_RESET);
//						HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_SET);
//						//printf("检测到黑线，切换到STATE4\n");
//					}
//					else
//						jy62_proc();
//			break;
//			case 4: //循迹状态2
//				if(offset == 100.0f)
//				{
//					uint8_t all_white = 1;
//					for(int i=1;i<=7;i++)
//					{
//						if(sensor_arry[i] != 0)
//						{
//							all_white = 0;
//							break;
//						}					
//					}
//				
//					if(all_white)
//					{
//						path_state = 5;
//						//printf("ALL:%d\n",all_white);
//						buzzer_beep(2,300,100);
//						motor_speed_set(0,0);
//						stop_flag = 1;
//						HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_RESET);
//						HAL_GPIO_WritePin(GPIOB,GPIO_PIN_14,GPIO_PIN_SET);					
//					}
//				}
//				else
//				{
//					base_speed= 350 - fabs(offset) * 0.5;
//					if (base_speed < 200) base_speed = 200;
//					pid_output = PID_Calc(&sensor_track_PID, 0 , offset,0);
//					L_P = base_speed - pid_output;
//					R_P = base_speed + pid_output;
//					motor_speed_set(L_P, R_P);
////					printf("%d,%d\n",L_P,R_P);
//					return;				
//				 
////				printf("%d,%d\n",L_P,R_P);
//			break;
//			case 5:

//			break;
//			
//		}
//	}
//	if(tracking_move == 3) // 8字环绕
//	{
//		Yaw_target = 
//	}
}











